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RECUBSP/E  FILTERING  ALGORITHMS  FOR  SHIP  TRACKING 


INTRODUCTION 

KYigting  naval  forces  can  be  deployed  and  concentrated  more  effectively  with  a better 
knowledge  of  current  and  probable  future  locations  of  the  surrounding  shipping.  Making 
good  use  of  available  surveillance  assets  in  performing  this  tracking  function  requires  the 
correlation  of  data  coming  from  many  sources  at  unpredictable  times.  Because  of  the 
large  volume  of  such  data,  there  is  interest  in  creating  a capability  for  automatic  ship 
tracking  using  sporadic  and  noisy  observations  of  position  only.  This  report  investigates 
the  use  of  certain  recursive  filtering  techniques,  in  p>articular  ones  based  on  Kalman  filters, 
for  this  purpose.  This  work  provides  alternatives  to  existing  algorithms  for  possible  use 
in  automatic  ship  tracking. 

The  ship  tracking  algorithms  were  designed  for  these  anticipated  uses: 

Track  Geieration  (Kalman  Filter) 

• Estimation  of  present  location 

• Prediction  of  future  locations 

• Generation  of  **gates”  (position  confidence  regions)  for  re port-to- track  cor- 
relation at  present  time  in  a multitarget  environment. 

Track  Smoothing  (Bayesian  Smoother) 

• Generation  of  “gates”  for  report-to-track  correlation  at  previous  times  (i.e., 
for  out-of-sequence  reports)  in  a multitaigct  environment 

• Estimation  of  previous  locatioiu  (i.e.,  track  history). 

As  input  data,  these  algorithms  require  reports  specifying  time  of  observation,  observed 
ship  position,  and  a covariance  matrix  for  the  errors  in  the  observed  position  (or  equiva- 
lent information  in  the  form  of  a confidence  r^on,  containment  ellipse,  etc.).  If  an  ob- 
served velocity  is  also  reported  at  a given  obsairation  time,  it  also  must  be  accompanied 
by  a corresponding  error  covariance  matrix  in  order  to  be  utilized.  Because  of  the  adapt- 
ive nature  of  the  Kalman  filter  used  for  track  generation,  no  additional  information  (such 
as  estimated  heading,  speed,  or  maneuverability)  need  be  specified  extenudly,  and  a track 
can  be  with  a single  observation.  The  time  intervijs  between  successive  observa- 

tions may  be  variable.  The  input  reports  are  norroally  processed  recursively  in  their  nat- 
ural time  sequence  and  need  not  be  recalled  after  their  initial  use.  The  incorporation  of 
an  out-of-sequence  report,  however,  requires  that  the  Intervening  reports  be  available  for 
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reprocessing.  As  output,  these  algorithms  provide  (a)  estimates  of  ship  position,  at  present 
or  future  times  for  the  track  generator  and  at  past  times  for  the  smoother,  (b)  error  co- 
variance  matrices,  or  equivalent  containment  eUpses,  that  correspond  to  th^  estimates. 
The  track  generation  algorithm  also  provides  estimates  of  average  velocity  (two  compo- 
nents) and  maneuverability  (two  parameters),  which  are  revised  after  the  receipt  of  each 
observation. 

There  are  two  salient  features  of  these  particular  tracking  algorithms.  First,  they  are 
based  on  a continuous-time  ship  motion  modeL  This  feature  allows  observations  that 
are  unevenly  spaced  in  time  to  be  processed  in  a statistically  consistent  manner  by  the 
track  generator.  It  also  enables  the  smoother  to  consistently  interpolate  the  tracks  for 
processing  out-of-sequence  observations.  Second,  the  ship  motion  is  approximated  in 
this  model  as  the  vector  sum  of  a constant  average  velocity  and  a two-dimensional 
Brownian  motion.  These  two  velocity  terms  are  processed  concurrently  but  separately 
by  the  track  generation  algorithm.  A standard  Kalman  filter  is  used  to  estimate  the  average 
velocity  with  the  position.  Another  recursive  procedure  is  used  to  estimate  the  intensity 
statistics  of  the  Brownian  motion  from  ttie  “residuals"  of  this  Kalman  filter.  These 
estimates  then  are  used  as  “driving  noise"  parameters  in  the  Kalman  filtor  to  adaptively 
modify  its  subsequent  operation.  The  purpose  of  this  adaptive  modification  of  the  basic 
Kalman  filter  algorithm  is  to  make  it  flexible  enough  to  track  a wide  variety  of  ship 
motions  without  prior  external  specification  of  the  motion  type. 

I The  track  generation  algorithm  operates  recursively  in  time.  Basically  it  propagates 

the  track  forward  between  observations  by  dead  reckoning  and  updates  it  whenever  a 
i ' new  report  is  received.  The  track  smoothing  algorithm  operates  recursively  in  reverse 

time  using  the  output  of  the  track  generator  (position  estimate  and  covariance  matrix)  as 
input.  These  two  algorithms  are  rirst  developi^  here  for  tracking  on  a planar  surface. 
Then  they  are  extended  to  tracking  on  the  surface  of  a sphere,  both  in  geographical  co- 
ordinates of  latitude  and  longitude  and  in  three-dimensional  rectilinear  coordinates.  The 
algorithms  for  the  planar  case  are  implemented  as  experimental  Fortran  programs  and 
I-  tested  on  both  realistic  and  idealized  ship  tracks. 


SOME  COMMENTS  ON  RECURSIVE  FILTERING  TERMINOLOGY 

Let  X be  a state  vector  describing  a ship’s  location  such  that 

i(f)  = F{t)x(t)  + w(t) . (1) 

where  F is  a matrix  time  function  and  w is  a Gaussian  white  noise  process  with  mean 
u}(t)  and  (known)  normalized  covariance  matrix  Q(f).  This  normalization  refers  to  the 
limiting  value  of 

^ £|[w(t  + A)  - w(t  + A)  - u;(f)  + c5(f)l lie(t  + A)  - w(t  + A)  - w(t)  + u»(01^|. 

The  state  vector  x will  contain  two  position  components  defining  the  ship’s  location,  and 
possibly  other  components,  such  as  velocity,  as  welL  At  sporadic  times  f,-,  i = 0, 1,  2, .... 
noisy  observations  z,-  of  x(t,)  are  received  such  that 
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+ rii , (2) 

where  u i matrix  and  {Rj}  is  a sequence  of  independent  random  vectors  such  that 

nj  is  Normal  [0, 

and  is  statistically  independent  of  w.  The  observation  times  are  ordered  such  that 
i>  tj  but  ate  othervrise  arbitrary.  Also,  a prior  probability  distribution  is  as- 

sign^ to  the  state  vector  at  the  initial  observation  time  such  that 

x(to)  is  Normal  . (3) 

For  any  pair  of  times  t and  7 such  that  T > t > tQ,  the  conditional  probability  dis- 
tribution of  x(t).  givoi  the  prior  distribution  anJ  all  the  observations  contained  in  the 
interval  [toi  ^ t happens  to  be  multivariate  Normal.  The  moments  A>fj>rmining  this  dis- 
tribution are  dented  as  follows. 

Definitions 

t](T,t)  = £[x(t)J  given  all  data  in  tto»^ 

^(T.  t)  = £|(x(t)  - tj(T,  t)l  [x(t)  - v(T,  f)l^}  given  all  detain  (to,  TJ 

x(t)  = ri(U  t)  « £[x(t)l  given  all  previous  data 

P(t)  - = £ [x(f)  - x(t)][x(t)  - x(t)J^  given  ail  previous  data 

Capital  letters  denote  matrices,  and  lower  case  ones  vectors;  A'^  denotes  the  transpose  of 
Ai  E denotes  expected  value. 

The  Kalman  filter  corresponding  to  Eqs.  (1),  (2),  and  (3)  generates  statistics  re- 
cursively from  the  observations.  These  startles  are,  at  any  time  t,  an  estimate  of  the 
state  vector  x(0  and  an  error  covariance  matrix  for  this  estimate.  These  statistics  have 
the  propoty  ttut 


x(t)  = filter  estimate  of  x(t) 
and 


Pit)  = filter  error  covariance  matrix  for  x(f) . 

Hriioe,  this  Kalman  filter  may  be  regarded  as  a real-time  conditional  probability  computer. 
Time  t may  be  at  or  between  observatioa  times. 

Another  recursive  algorithm,  called  the  Bayesian  smoother  for  Eqs.  (1)  tbrouf^  (3), 
can  be  used  in  conjunction  with  the  Kalman  filter  algorithm  to  compute  t}(r,  t)  and 
K{T,  t).  The  details  of  both  algorithms  are  shown  in  Appendix  A.  *7(7,  t)  is  called  the 
smoothed  estimate  of  the  state  vector  x(t)  at  time  7,  and  £(7,  t)  is  cdlri  the  error 
covariance  matrix  of  ?7(7,  t). 
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In  the  limiting  ca^e  where  det  [A/^]  -*■  «>  for  the  prior  distribution  of  x(to)  (i.e.,  a 
“flat  prior"),  rj(T,  t)  and  K(T,  f)  may  be  interpreted  as  the  first  and  second  moments, 
respectively,  of  the  normalized  likelihood  function  (i.e.,  integrates  to  unity)  of  x{t)  for 
the  observation  values  received  and  the  motion  model  postulated.  This  normalized  like- 
lihood function  is  also  multivariate  Normal.  No  prior  distribution  for  xHq)  is  involved  in 
this  interpretation,  which  includes  the  Kalman  filter  statistics  x(t)  and  P(t)  as  special 
cases  of  t2(T,  t)  and  K(T,  t). 


UNDERLYING  SHIP  MOTION  MODEL 

The  tracking  algorithms  are  based  on  the  Kalman  filter  and  Bay<%ian  smoother  for  a 
specific  motion  model  in  which  a ship’s  motion  is  approximated  ios  the  vector  sum  of  a 
constant  (average)  velocity  and  a two-dimensional  (random)  Brownian  motion.  The  in- 
tensity of  the  Brownian  motion,  which  is  actually  specified  by  three  independent  param- 
eters, is  selected  to  correspond  to  the  extent  of  maneuvering  performed  by  the  ship  with 
respect  to  a constant-speed,  great-circle  course.  This  particular  motion  model  was  selected 
as  a basis  for  these  tracking  algorithms  for  'he  following  reasons. 

• The  general  recursive  filtering  algorithms  of  Appendix  A reduce  to  particularly 
simple  forms  for  this  model  if  t^e  earth's  curvature  is  neglected.  Modifications  to  account 
for  tl  *s  curvature  are  also  relatively  simple. 

• The  motion  model  has  sufficient  flexibility  to  give  at  least  a rough  approxima- 
tion to  a wide  variety  of  ship  motions. 

• The  smoothed  tracks  generated  are  great  circles  between  smoothed  observation 
points. 

• Unevenly  spaced  observations  can  be  accommodated  in  a systematic  way. 

• Tracks  can  be  initiated  with  a single  observation,  so  no  qualitative  distinction 
between  tracks  and  unassociated  observations  is  necessary.  Track  initiation  and  observation- 
to-track  association  can  be  regarded  as  special  cases  of  track-to-track  association. 

• The  Unear  size  of  the  containment  ellipse  generated  by  the  corresponding  Kalman 
filter  (i.&,  the  track  propagation  algorithm)  often  grows  only  as  the  square  root  of  the 
tir'c  elapsed  since  the  last  observation.  Since  the  gates  used  in  observation-to-track  asso- 
ciation ^orithms  often  correspond  roughly  to  these  containment  ellipses,  this  is  possibly 
an  important  element  in  achieving  good  observation-to-track  association  performance  with 
sparse  observations  at  high  shipping  densities. 

The  execution  of  the  tracking  algorithms  based  on  this  model  requires  that  each  re- 
port of  a ship’s  location  specify  the  time,  the  observed  position,  and  the  (2  X 2)  covariance 
matrix  of  the  observation  errors.  The  average  velocity  and  Brownian  motion  intensity 
parameters  are  estimated  from  the  observation  data  and  need  not  be  specified  externally. 

As  output,  the  algorithms  are  capable  of  providing  the  following  information  about  a ship 
at  any  given  time. 
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Ttack  Generation  Algorithm  (Kalman  Filter) 

• Estimates  of  current  position  and  average  velocity  (four  components 
altogether) 

• A (4  X 4)  covariance  matrix  for  the  errors  in  these  estimates 

• An  estimate  of  the  three  Brownian  motion  intensity  parameters  describing 
the  ship’s  maneuvering. 

Track  Smoothing  Algorithm 

• Estimates  of  position  at  any  past  time 

• A (2X  2)  covariance  matrix  for  the  errors  in  these  estimates. 

The  information  concerning  the  ship’s  position  is  the  output  of  primary  importance. 
Estimates  of  average  velocity  and  maneuvering  are  included  chiefly  for  the  ulterior  pur- 
poses of  estimating  future  and  past  positions. 

This  motion  model  is  tailored  for  tracking  with  position-only  observations.  It  is  also 
possible  to  incorporate  independent  velocity  observations  into  the  tracking  procedures, 
but  this  is  not  a completely  straightforward  extension.  The  difficulty  is  basically  that  a 
sliip’s  velocity  in  this  model  has  two  components— an  average  velocity,  which  is  being 
estimated,  and  a completely  random  velocity,  which  is  not.  \^hat  is  observed,  however, 
is  the  sum  of  these  two  components;  thus  some  additional  assumption  must  be  specified 
about  the  relation  of  the  observed  velocity  to  the  constant-velocity  component  of  the 
model.  Possible  procedures  for  making  such  a modification  are  discussed  in  Appendix  B. 


TRACKING  OF  PLANAR  MOTION 

It  is  convenient  to  begin  the  detailed  development  of  these  tracking  algorithms  with 
the  consideration  of  a special  case.  In  this  case  a ship’s  motion  is  restricted  to  a portion 
of  the  earth’s  surface  which  is  small  enough  to  be  adequately  approximated  by  a plane. 
The  resulting  algorithms  are  thus  easier  to  understand  and  can  e^y  be  generalized  to 
algorithms  for  tracking  on  a sphere.  In  fact,  this  generalization  is  basically  just  a matter 
of  rotating  the  coordinate  axes  at  each  time  of  interest,  usually  an  observation  time,  to 
realign  the  y axis  with  local  north. 

In  this  planar  context,  an  approximation  of  the  ship’s  motion  is  described  by  a state 
vector  consisting  of  two  rectangular  position  coordinates,  x and  y,  which  satisfy  the  dif- 
ferential equation 


where 
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is  a two-dimr'nsional  Gaussian  white  noise  process  v^rith  constant  mean 


u 

— (the  ship’s  average  velocity)  and  constant  normalized  covariance  matrix  Q 
V (representing  maneuvering  about  this  average  velocity)  such  that 
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It  is  assumed  that  bias  has  been  removed  from  the  position  observations  so  that  the  obser- 
vation at  time  tj  can  reasonably  be  approximated  as  the  2-vector 
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is  a zero-mean  bivariate  Normal  random  variable  with  covariance  matrix  i2,- 
such  that 
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The  Q and  matrices  have  been  kept  deliberately  in  general  two-dimensional  form.  No 
significant  computational  reduction  appears  to  be  possible,  unless  the  observation  errors 
are  assumed  to  be  statistically  independent  in  those  rotated  coordinates  which  also  diag- 
onalize the  Q matrix.  Aithough  such  an  assumption  may  be  reasonable  in  some  situa- 
tions, it  might  constitute  a serious  inefficiency  in  the  use  of  the  data  when  the  “error 
ellipses’’  of  successive  position  observations  are  long  and  narrow  and  differ  widely  in 
orientation,  which  is  a case  of  major  interest  here. 


Track  Generation— Adaptive  Kalman  Filter 

Althou^  a Kalman  filter  for  Eqs.  (4)  and  (5)  could  be  constructed  directly,  it  could 
not  be  implemented  in  practice  for  position  estimation  because  the  average  velocity  com- 
ponents u and  V and  the  Brownian  motion  intensity  matrix  Q are  not  known.  They  must 
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be  estimated  from  the  observation  data  as  the  track  is  being  generated.  It  is  assumed, 
however,  that  the  covariance  matrix  of  each  observation  error  is  known  to  the  tracker 
(i.e.,  both  the  observed  position  and  error  covariance  matrix  are  necessary  parts  of  the 
input  data). 

If  the  average  velocity  components  are  adjoined  to  the  state  vector  of  the  ship  mo- 
tion model,  the  motion  can  be  described  in  terms  of  the  augmented  state  vector  by  the 
differential  equation 
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where  Wx  and  Wy  now  denote  zero-mean  noise  components  and  Q denotes  the  correspond- 
ing 2X2  partition  of  the  4X4  driving  noise  matrix,  the  other  components  of  which  are 
zero.  Furthermore,  the  position  observations  can  also  be  expressed  in  terms  of  this  aug- 
mented state  vector  as 
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Hence,  if  Q were  known,  u and  v could  be  estimated  concurretitly  with  x and  y by  the 
use  of  the  Kalman  filter  corresponding  to  Eqs.  (6)  and  (7).  As  an  ad  hoc  procedure  based 
on  this  concept,  this  filtering  algorithm  is  lised  as  if  Q were  a known  constant  matrix, 
except  that  the  value  of  Q used  in  these  computations  is  updated  at  each  observation 
time  by  another  recursive  algorithm.  The  initial  estimates  of  u,  c,  and  Q are  all  taken  as 
zero,  which  in  effect  gives  priority  to  estimating  the  average  velocity  (u,  v)  over  estimating 
the  “maneuvering  matrix”  Q. 


The  Basic  Kalman  Filter 

If  Q is  treated  as  a known  constant  for  the  moment,  the  evolution  of  the  conditional 
mean  and  covariance  matrix  of  the  state  vector  between  generic  observation  times  t,-  and 
ti.^1  can  be  described  by  specializing  the  results  of  Appendix  A to  Eqs.  (6)  and  (7),  giving 
the  following  differential  equations: 
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i 


r 

f, 


1 


I 


X =■  u 

9 = V 

i = 0 
0=0 

Pxx  ~ ^Pxu  9xx 
Pxy  ~ Pxv  Pyu  ^xy 

Pyy  ~ 2pyv  + Qyy 

Pxu  ~ Puu 
Pxv  ~ Puv 
Pyu  “ Puv 
Pyu  = Puv 

Puu  = 0 

Puv  = 0 

Puv  = 0. 

In  this  case,  the  differential  equations  can  be  integrated  analytically  to  give 
*(^r+l)  = x(tt)  + Tu(tf) 

y(ti>i)  = y(tt)  + 

«(fr+i)  u(t^) 

5ar+i)  = out) 

^xx  ~ Pxx^^i^  9**^ 

^xy  ~ Pxy(^i)  ■*"  Pyu^^t)^"^  Puv^^t^^^  '*’  ^xy'^ 

rtiyy  = Pyyitf)  + 2pyuitt)r  + Pu„(tf)T2  + 

^xu  ~ Pxu(^t^  Puu(^t)^ 

mxu  = Pxvi^i''  + Puv(^i)‘>’ 

rtlyu  = PyuUr)  + Puv(*t)'r 

”^yv  = Pyvi^t)  * Pw(ti)'r 

m^u  = Puu(^t) 

m^v  = Puv^tf) 

niuu  = Pw(ti) . 
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(8) 

(9) 

(10) 

(11) 

(12) 

(13) 

(14) 

(15) 

(16) 
(17) 
(13) 

(19) 

(20) 
(21) 


I 


1 

i 
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where  r = - t,-,  and  the  m variables  denote  the  corresponding  components  of  the 

P matrix  at  time  After  the  observation 


rzx(i  +1)1 

L^yO+Dj 


at  time  the  conditional  mean  and  covariance  matrix  components  are  updated  ac- 
cording to  the  equations 


[i  +1)  - Xi  - TUi 


Cj+l  Xi  Ui  Pxx  I Pxy  , + 

= +7  ___  + , 

?i+l_  _ yi  _ _ J Pxy  1 Pyy  -SyO' + 

l_  1 _l/+l  I— 


1)  - y,  - TVi 


Ui+ri  ”1  Vpxu  ! PyM~l  , r2jc(«  + 

___  = ___  + ___| 

U|+lJ  |_  Vi  J I^Pjck  I PyuJ^.^^  ^yii  + 


1)  - i/  - TUi 


1)  - yi  - TVi 


where  the  integer  subscripts  t and  i + 1 indicate  the  value  of  variables  at  tf  and  tf+i  and 
the  P matrix  components  at  are  computed  from  the  equation 


Pvu  J L 
i+1 


ntxx  + rxxii  +1)  j m^y  + r*y(f  +1) 
ff*xy  + <'xy(i  +1)  I fnyy  + ryy(i  +1) 


rrixx  I ftixy  I rrixu  • 

y 1.  _ .1 1 

t t 1 

RIjcy  I tHyy  j flTyu  | niyff 


Once  the  initial  conditions  are  specified,  Eqs.  (8)  through  (24)  define  the  track  gen- 
eration procedure  imder  the  assumption  of  known  Q.  A convenient  practice  for  initiating 
this  procedure  is  to  start  tracking  at  time  immediately  after  the  first  observation,  with 
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^(^o)  ~ ^xo 
y{to)  = Zyo 

u(f$)  = 0 
Htl)  = 0 

Pxxi^o)  - ^xxo 
Pxy(to)  = r^yo 

Pyy(to)  - ^yyo 

Puado)  = P«,(^0) 


(25) 


(26) 


all  other  components  of  P(to)  = 0 , 

where  V is  the  (externally  specified)  average  speed  of  ships  covered  by  the  tracking  sys- 
tem. It  is  possible  to  avoid  specifying  a value  of  V by  using  (to)  = Pu,(to)  which 
corresponds  to  the  use  of  a fiat  prior  for  the  initial  state  vector  to  generate  its  normalized 
likelihood  function.  This  modification  would  make  the  tracking  somewhat  less  efficient, 
however,  and  is  probably  needless  since  a reliable  estimate  of  V,  or  at  least  a finite  upper 
bound,  would  usually  be  available. 


Adaptive  Modification  for  Recursive  Estimation  of  Q 
To  account  for  unknown  Q,  note  that  tbe  term 


ZxU  +1)  - Xi  - TUi 

ZyU  +1)  - y,-  - TVi 


in  Eqs.  (22)  and  (23)  can  be  expressed  as 


Xi  - Xi”|  Fu  - u,  ”1  fn^Ci  +l)n 

|+t1 [+1  u/df+l t 

_yi  - L»y(^+l)J 

which,  if  Q were  known,  would  have  a zero  mean  and  covariance  matrix 


[: 


mxx  j Wjcy  I 
m^y  I niyy  I 


[TQxx  I rr«(»+l)  I ^‘*y(»-H)'j 

TQxy  1 »’«yyj  [j*y(‘ +1)  I »’yy(» +1)J 
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Fuiihermoie,  if  Q were  known  eind  used  in  Eqs.  (8)  through  (24),  the  vectors 


~€x(i  + 1)“ 

“zjc(i  +1)  - Xi  ~ TUi  ~ 

+1)_ 

_Zy(i  +1)  - yj  - TUi  _ 

would  be  statistically  independent,  in  which  case  a reasonable  statistic  $(t  + 1)  for  ap- 
proximating Q at  time  would  be  given  by  the  equations 


and 


4^(i  - 1)  = T-^  Y!.  r ~ ~ 


1 ^ 1 
= rri  H T:  K'^yi  ■ P*y^^7 >"  '■*y;l 


1 i+1  1 

+ 1)  = fn.  ^ y [^y/  ” -Pyy^^y  ^ *"  '’yy/J  » 


where  r/  denotes  fj  - fj-i . These  statistics  cannot  be  used  directly  to  estimate  Q because 
Q is  needed  to  compute  the  e*s  and  p’s.  However,  these  statistics  obey  the  following  re- 
curaion  equations: 


«**(»  + 1) 


- * / ^ ^ 1 [cj(<  -*-  l)  - +1)  ^ 

^ (i  + 1)  . T ~ 9**(0 


(27) 


q,y(i  + 1)  » q^y(i)  . ^ 

and 


I fe*(i  +l)€y(»  +1)  - m^y  - r^y(i  + 1)  1 

+ (HT)  L ^ 


(28) 


, re5(i  +1)  - niyy  - ryyd  +1)  ] 

5„(i  + 1)  • f . 


(29) 


where  r,  ntxxt  ntxy  and  rriyy  are  defined  as  in  E<]^.  (8)  throu^  (24).  As  an  ad  hoc  pro- 
cedure, Eqs.  (27)  throu(^  (29)  are  used  recursively  to  generate  estimates  of  the  compo- 
nents of  Q,  starting  with  ^(o)  - ^y(o)  = Qyy(o)  = 0.  It  is  also  desirable  to  constrain 
these  estimates  so  that  they  form  a positive  semidefinite  matrix  which  is  diagonalized  by 
a rotation  to  coordinates  aligned  wi^  the  estimated  average  velocity  vector  (i.e.,  maneu- 
vering is  assumed  symmetric  about  the  ship’s  average  heading).  One  way  which  has  been 
found  to  accompli^  this  is  to  use  the  following  as  estimates  for  the  time  interval  (t,*,  ) 

iu  the  context  of  Eqs.  ,8)  through  (24): 
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and 


where 


^xx  ~ 


= 


1 

2 


I* 


«?  * e?  J 


«f  ♦»? 


{ = max  {0,  + §yy(i)]} 

a?  * of 


i if 


• • ■ Vm 

«««  > £ 

uf  + of  ^ 

q^yii)  otherwise. 


X = < 


(30) 


(31) 


(32) 


(33» 


(34) 


The  justification  for  this  procedure  is  given  in  Appendix  C. 

Final  Algorithm 

This  completes  the  sr>ecification  of  the  recursive  track  generation  procedure.  To  sum- 
marize this  procedure,  hacking  begins  immediately  after  the  initial  observation  at  time  Iq  with 
initial  conditions  given  by  Eqs.  (5),  (25),  (26),  and  ^(o)  ==  ^y{o)  = ^y(o)  = 0.  From 
time  tf  to  time  tUu  i = 0,  1, ...,  the  track  is  generated  as  follows; 

Track  Propagation 

• Generate  qxx*  9x>i  9yy  Orom  ^(i)>  icyO),  ^y(0  with  Eqs.  (30)  through  (34). 

• Use  these  values  in  Eqs.  (8)  through  (21)  to  generate  ^(ff^.^),  y(f,>i), 

«(f»‘+l).  «5(fr+i).  and  the  m’s. 

Track  Updating 

• Use  these  values  of  the  m's  in  Eqs.  (22)  throu^  (24)  to  generate  the  new 

estimates  £|-+x,  Si+i,  the  observations  Zj^(i  1),  Zy(i  + 1). 
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• Use  Eqs.  (27)  through  (29)  to  compute  * !)•  4c>(<  ^)>  ^>0 

where  (as  defined  earlier) 


£,(i  +1)  = Z^(i  +1)  - Xi  - TUi 
€y(i  V 1)  = Zy(i  + 1)  - y,  - TV, . 

Note  that  track  initiation  is  accomplished  with  only  one  position  observation  in  this 
algorithm.  Hence  an  unassociated  observation  can  be  regarded  as  a one-point  track.  In 
this  regard,  it  is  perhaps  helpful  to  consider  a track  as  consisting  of  the  time  history  of 
the  conditional  mean  and  covariance  matrix  of  the  entire  state  vector,  not  just  the  time 
history  of  x and  y.  With  this  interpretation,  no  qualitative  distinction  between  report-to- 
track  association  and  track-to-track  association  is  necessary.  The  implementation  of  this 
algorithm  requires  that  a total  of  17  quantities  be  carried,  propagated,  and  updated  for 
each  ship  being  tracked  (i.e.,  the  estimates  jc,  y,  u,  v,  the  10  independent  “p”  compo- 
nents of  the  corresponding  covariance  matrix,  and  the  maneuvering  parameter  estimates 
fljoc.  4xy>  and  qyy). 

In  actual  operation,  it  might  be  well  to  reinitialise  this  tracking  algorithm,  perhaps 
at  the  discretion  of  a human  operator,  if  a sequence  of  consistently  large  residuals  and 
€y  are  encountered  for  a given  ship;  such  an  event  would  imply  an  abrupt  change  in  ma- 
neuvering behavior.  Another  possibility  would  be  to  limit  the  i + 1 factor  in  Eqs.  (27) 
through  (29)  to  some  maximum  to  prevent  the  estimates  of  Q from  depending  too 
heavily  on  old  observations. 

Prediction  of  Future  Positions 

Although  the  preceding  algorithm  is  contemplated  mainly  for  the  updating  of  posi- 
tion estimates  after  the  receipt  of  an  additional  observation  (one-point  updating),  it  can 
also  be  used  for  computing  the  conditional  probability  distribution,  given  all  currently 
available  observation  data,  of  a ship’s  position  and  velocity  at  a future  time.  If  t,-  is  the 
time  of  the  last  observation,  this  can  te  done  with  the  track  propagation  steps  of  the 
above  algorithm  by  regarding  the  future  time  in  question  as  ff+i-  The  conditional  dis- 
tribution is  then  Normal  with  mean  (predicted  position  and  velocity) 


yur+i) 

“(fr+i) 


and  covariance  matrix  M,  as  defined  by  Eqs.  (12)  through  (21).  Aside  from  its  obvious 
tactical  value,  this  information  can  also  be  used  for  the  construction  of  position  and/or 
velocity  gates  in  observation-to-track  correlation;  in  the  latter  use,  is  the  time  of  the 
observation  possibly  being  correlated.  In  either  case,  however,  unless  a new  observation 
is  nrtnally  used  to  Update  the  track,  the  ’Htack  updating*'  steps  are  not  performed  and 
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any  su  baequent  track  ^.r^pegation  or  updating  proce^  from  time  as  if  thgao  computa- 
tions liad  never  taken  plam. 


Track  Smoothing 

In  addition  to  keeping  track  of  the  conditional  distribution  of  a ship's  current  posi- 
tion and  velocity,  it  is  occasionally  useful  to  know  the  cturent  distribution  of  its  state 
vector  at  a previous  time  as  well  (i^.,  the  smoothed  track  statistics  tj  and  K).  The  main 
use  foreseen  for  this  information  is  in  observation-to-track  association  for  out-of-sequence 
observations.  A smoothed  track  can  be  considerably  more  precise  in  practice  than  the 
past  history  of  the  track  generated  by  the  corresponding  Kalman  filter.  This  extra  preci- 
sion would  enable  out-of -sequence  observations  to  be  correlated  to  tracks  more  accurately 
in  areas  of  hi^  shipping  density. 

This  track  smoothing  algorithm  is  the  specialization  of  the  generic  Bayesian  smoother 
of  Appendix  A to  the  particular  ship  motion  model  adopted  above.  As  a simplifying  ap- 
proxinmtion,  however,  it  is  assumed  that  the  current  estimates  of  the  velocity  and  ma- 
neuvering parameters  u,  u,  qxxt  Qxyt  *nd  gyy  at  the  time  of  smoothing  are  the  exact  values 
of  theu  quantities— with  one  exception.  The  estimates  Qjtjc,  q*y,  and  $yy  are  first  adjusted 
according  to  Eqs.  (30)  through  (34)  to  insure  that  the  resulting  “maneuvering”  matrix,  de- 
noted by  Q,  is  positive  semidefinite.  Then  Q is  further  modified  to  compensate  for  the 
imcertainty  in  the  velocity  estimate,  which  is  suppressed  by  the  assumption  that  u and  0 
are  precisely  known  constants.  It  has  been  found  by  numerical  experimentation  that  this 
modification  can  be  achieved  reasonably  well  by  replacing  Q with  a matrix  Q such  that 


Q = Q + (T-tg) 


“ iU(T) 

^uv(T) 

_Pw,(T) 

PvvC^ 

(35) 


where  T is  the  time  of  smoothing  and  (q  is  the  time  of  track  initiation.  This  simplification 
makes  it  possible  to  use  the  Bayesian  smoother  corresponding  to  Eqs.  (4)  and  (5),  rather 
than  to  Eqs.  (6)  and  (7),  a reduction  from  four  state  variables  to  two  To  implement 
this  smoother  at  time  T requires  that  the  quantifies  *(ft),  y(t^),  Pxxiti),  PxyHt),  and 
Pyy(fj- ) be  available  for  all  observations  times  such  that  tj  < T. 

For  clarity  of  notation,  the  components  of  q(T,  t)  are  denoted  here  by 


and  those  of  K(T,  t)  by 
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r fc«(o  I kxy(t)  1 

|_  kxy(t)  I kyy(t)  _j 


The  variables  17  and  K are  continuous  in  t and  obey  the  following  differential  equations 
between  genetic  observation  times  and  (7  is  considered  a fixed  parameter  here): 


4*^4  (.-[-3 


and 


where 


K = QP-^K  + KP~^Q  - Q , 

( X(t)  = JC(tt)  + 

y(t)  = y{tt)  * 

r Pxx^U  ) j PxyiU  ) 

P(f)  I +(t-t,)Q 

l^Pxy(ft)  1 Pyy(4)_ 


(36) 


(37) 


is  the  Kalman  filter  solution  corresponding  to  Eq.  (4).  Equations  (36)  euid  (37)  can  be 
int^prated  analytically  in  this  case  by  noting  that  the  quantities 


and 


P'^(K-P)P~^ 

are  constant  on  the  interval  (t,-,  Therefore,  by  continuity,  T;(f)  and  K(t)  can  be 

computed  for  any  t6(t,-,  t,>i)  in  terms  of  their  values  and  at  time  as 
follows: 


• Let 


= i*(frfi)  = P(0  + <?(f,>i  - f.) . 


(38) 
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and 


Compute 


and 


*i+i  = = *(<!*)  + “Ui-n  - ^i) 


yi*i  = y(^>i)  = y(K)  * 


p(0l  ,/ 

\_y(t)  \ y.>i  / 


(39) 

(40) 


(41) 


(42) 


Thus,  it  is  easy  to  compute  ri  and  K recursively,  starting  with 


V(T,  T)  = 


x(T) 

y(T) 


and  K(T,  T)  = P(T) 


and  using  Eqs.  (38)  through  (42)  on  the  interobservation  intervals  in  reverse  sequence. 
Note  that  setting  t = t,-  in  Eqs.  (40)  and  (41)  gives  t},-  and  K{.  Equations  (38)  through 
(42)  can  be  computed  component  by  component  by  first  defining 

»■  * ti*i  - U 


aod 


s = f - f, , 


then  computing 

Pi  ~ Pxx^^i)  + q^s , 
Pz  - P*y(f|  ) (/jcy®  » 
PZ  ~ Pyy^^i  ) 9yy*  • 


= Pxxi^t)  * <lxx^  • 
ni2  ~ Pxy^^i  ) 9jcy^  * 

m3  = Pyyiti)  qyyT, 
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x,.,.!  from  Eq.  (39), 


and 

y,+i  from  Eq.  (40), 
and  then  computing 


5(0  = i(0  + 


y(t)  = y(t)  + 


Plf«3  ~ P2"»2 

~ . P2^1  ~ Pl"*2 

2 

(*i-*-l  ■*i  + l)  ■*’  2 

mimg  - rrt2 

mimg  - mg 

P2^3  “ P3"*2 

~ . P3"»l  “ P2"*2 

(*i+l  *i+l)  + 2 

2 

mi  mg  - mg 

mjmg  - mg 

(>i+i  "y/+i).  (44) 


*jcjc(0  = Pjcjc(0  + {*«(0  + i)tPi"»3  “P2^2]^  2Afjc^(t,4.i)[P2mi  - PimJ 


( 1 

? 

mi  m3  - 

m| 

X Ipim^  - P2m2l  + feyy(t,  + i)lP2"»i  - Pi"»2]‘ 

ll^y(t)  = pjcy(t)  + {feji:*(t/  + i)[P::»l3  -P2"»2HP2"*3  “ P3"*2l  **y(^i  + l) 

X l(p2mi  -Pim2)(P2m3  -pgft’g)  + (piWg  -P2m2)(p3mi  -pgmg)) 

r ^2 


+ftyy(t/  + l)iP2f«l  -Pimgj'fpgmi  “ P2"*2l  ^ 


and 


*j«(0  “ Pyy(0  + {*«(^>l)lP2"»3  -P3"*2l^  ■*■  2*;(y(0+l)[P3"*l  -P2'»2l 


X IP2"»3  " P3"*2l  *yy(0  + l)lP3'”l  " P2"*2l 


“}  { 2 

•>  [mi  mg  - m| 


(45) 


(46) 


(47) 


Note  that  Eqs.  (43),  (44),  and  the  corresponding  Kalman  filter  equations  for  x and  y 
imply  that  the  smoothed  tracks  (x,  y)  for  this  type  of  motion  model  ^ve  constant  veloc- 
ities between  observation  times.  The  tracks  are  continuous,  but  there  are,  in  general,  dis- 
continuities in  the  velocities  at  the  observation  times. 


Numerical  Performance 

These  planar  tracking  algorithms  have  been  implemented  as  experimental  Fortran 
programs.  These  implementations  have  been  tested  on  both  idealized  and  realistic  ship  tracks. 
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Fig.  2— ConUimnent  «UipM»  «t  last  obaarvation  time  for  previous  positions  ou  realistic  ship  track 

TRACKING  ON  A SPHERE:  GEOGRAPHICAL  COORDINATES 


The  ship  tracking  algorithms  developed  in  the  preceding  section  can  be  extended  to 
include  the  ^ects  of  earth  curvature  if  it  is  assumed  that  this  curvature  is  negligible 
within  a ship’s  position  sigma  ellipse  generated  by  the  filter  or  the  smoother.  In  this 
case,  the  ship’s  motion  can  continually  be  approximated  by  Eqs.  (4)  and  (5)  in  local 
rectangular  coordinates.  Although  it  has  been  found  convenient  for  some  other  tracking 
algorithms  of  this  type  to  keep  these  local  x,  y coordinates  aligned  with  the  estimated 
velocity  vector  for  reasons  of  symmetry,  it  seems  simpler  here  to  keep  them  aligned  with 
local  north  because  the  motion  and  observation  models  are  fully  two-dimensional  anyway. 
This  section  contains  an  extension  of  the  planar  filtering  and  smoothing  algorithnos  to 
tracking  on  a sphere  when  a ship’s  location  is  described  in  geographical  latitude  and  longi- 
tude coordinates.  Alternate  algoriti  t ns  are  developed  in  the  following  section  for  tracking 
on  a sphere  in  rectilinear  coordinates,  which  have  certain  computational  advantages. 


Track  Generation 


I 


The  basic  procedure  using  the  rectursive  filter  is  to  perform  the  track  propagation  step 
with  dead  reckoning  along  a great-circle  path  using  the  estimated  average  velocity.  Track 
updating  is  accomplished  by  first  establishing  a rectangular  coordinate  system  centered  at 
the  current  propagated  position  and  aligned  with  local  north,  then  updating  as  in  the  sec- 
tion ’’Tracking  of  Planar  Motion,”  and  finally  computing  the  latitude  and  longitude  of  the 
updated  position.  Between  successive  observations,  say  at  times  tj  and  the  details  of 
this  proc^ure  are  as  follows. 
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READ  parameter  VALUES 

M »;Ut.'fi£R  OF  DF.TECTlOfjS  (=  f;0»  OF  DATA  CARDS) 
VELVAR  = PRIOR  SPEED  VARiANCE 


5071 


READ  5071»fttVELVAK 
FORMAT) l3tFlS.5) 


C 

C 

C 

C 

c 

c 

c 

c 

c 


c 

c 


READ  ) Ai\r  STORE)  DATA  TOR  EACH  OETcCTICW 

T = TIME  , . 

AaiAY  X OBSFRVFO  LOCATIO*.  CCORDIHATE^ 

SMA  * Sr“I(  AJOR  AXIS  CF  86  PtKC£-“.T  CCf.TAIRMERT  ELLIPSE 
for  oaSERVATIOfi 

SKI  • sc(''.ii'if;0R  AXIS  OF  c>‘kT All-.; :ert  ellipse 
ThT  m OR  IEMTaT  lO'l  OF  SE'-I  MAJCR  AXIS  (DEGREES  CL0CL'>''1S£ 
FKCf-i  Y-AXIS) 

5^70  format (6F1- •A) 
no  9 I*I»M 

RffAO  507Cj»T  ( I ) »AX(  I ) t/.Y(  I ) »S  iA»;,.'lI  *TnT 
ThT»ThT/57.3 

ARXX(  I )»<  (S;iA*SIN(  lMTU«*2+(S..IiC0S(THT)  )ii*2)/6. 

Af>Y(  I ) aSIN  I ThT  )*CUo(  T»<r  J (;>.  iA5X;A-SMI  « j.-.l  ) /4. 
o AKYY(  I)s(  (SMI«SIN(  ThT  ) )i**?<-(Si:A”C('Sl  IJiT  ) )*-2)/4» 

imtialuation 

xx(i)=Axm 
YY(l)-AYm 
PXX(1)=APXX( 1) 

PXY<i)«A?XY(U 
PY¥( 1 )=A?YY( 1 ) 

C1»PXX(1)+PYY(1) 

r2»SCRT(  (PXX(i)-PYY(i)  )«*2♦A.•-PXYIl)^^•2^ 

C1=.5*(C1+C2) 

C2«C1-C2 

S-!AJ(1)=2.*S0RT  (Cl) 

SMIN(1)=2«»SGRT (C2) 

TH(l)=57.3»ATA.'i(  tPXX(l)-Cl)YPXY(U 

OXXau« 

OXY«U. 

QYV-0. 

U»0. 

VsU. 

PXOuO# 

PXVaO, 

PYt^av/. 
pyvai,, 

PUU«,5*V£LVAK 
PUVao, 

Fig.  3— FVogtam  Ukting 
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PVVsPUU 
CX A=0* 

CXV=J. 

CYV=0. 

RrcUf?SIVp  .ST.ilC  V£C><.'.<  TiTIMAT  lO.'J 

(JO  1 I = 2.i. 

^X=AX( 1 ) 
ilY  = AY(  I ) 

KXX»Ar\XX(  I ) 

RXY=ARXY(  I ) 

KYY  = AKYY( I I 
TAu=T(1)-T(I-1) 

XPAR*XX ( I-l ]+UfrTAt 
Yf»AR  = VY(  I-J  )+V-7Au 

<-.XX»PXX(  t-1  )+p.>PXb*I/.lj+Pui/i:TAlj«TAJ+OXXcTAU 
r,XY  = PXY<  i-i  ) + (PXy-t.p  fj)  i^TA^^+Pl-V^TAU*  TAu+UXYii'TAU 
GYY  = PYY(  i-i  |+2.i»PYV<HAU-fPYV»TAU«lAu+UYY*lALi 

r,XU»PXL+pUU*TAO 
<",XV  = PXV  + PUV»TA- 
r,YuapYO+poV*TAu 
GYVspYV+pVV*TAl.< 

r.f:T=  { c-XX+RXX  ) v ( r,Y  Y+R  Y V J - ( OX  Y^i-.XY  ) » » 2 
HXX=(GYY4'RYY)/()ET 
HXY=-(uXY+RXY) /DtT 
HYY=(GaX^.P,XX)/(iFT 

0XX(  I )=r,XX-GXX  ;GXX*'iXX-2.*^r,XX<tGXV JHAY-OXYfiGXVwHYy 
PXY(  j (=r.xY-GXX*GXY*HXX-(C.AX«0  « Y+C>AYt  GXY  ) anXY-GXY»GYY<-hYY 
OYY(  I)«r,YY-r.VY«GYY*i-|YV-2,-r,YV-G-YVJ:HXV-oX\*GXY*HXX 
Cl=PXX( . )+OYY( 1 ) 

C2  = S0RT  ( (PXX(  I 1-PVY(  I i 1 i*.2+&.:'PX^  d 1 X'2  > 

Ci=.5.*(CI+C2) 

C2»C1-C2 

jMA  J ( I > *2  •*SOf\T  ( C 1 I 

P'MN<  I )s2»*SC!<r  (C<?) 

T.H(  I )=S7,’*'ATAM(  ( PaX  ( (1  -t  1 1/PaY.  i ) 1+Ow. 
pAo  = GXU-GXA<GX,'i;  HXA-lGXJ  J.i'.'i  v.+G/>  r ) ^^•iA  ■> -C  X Y“GYo*‘H>  V 
p;.\/=rx\«-riAA”G''>Y“HAX-  ( gxx*(-vv+(.av  ^gac  j '■jmxv-gx'i  >*gy\(  ohyy 
•JYk>=f.YU-GV  yi^GYO»HVV-(  r v Yii  f:Ai,+r.AY»-r,^0  J •'MX  V-GXV  *GXU*Haa 
PYV  = GYV-gY  V^G>A'»HYV-(G^  y*^-f  AV  + .''.XYOGVY)  K'HX  Y-GX  VvgXV»HXX 
PU./*P0G-gXU  = GX;J*HXX-2.«Gav  vgu'^HaY  -GVv^kGYOvuYY 
•PUV=PUV-GXU*riXv'«H.'.X-lG>;i.'Jr'.  V-t-OYiKT./.V  J «HXV-GYueGYV»riVY 
PY'VaPVV-GXV*GA\  »HaX-2,  r-A>,  ■••G^  i -GV  V«GYV):MY  > 

PFT«RXX#RYY-RXYiJRXY 
HXX=RYY/nET 
HXY=-RXY/nFT 

hyy=rxx/det 

XX  ( I ) = A^‘AR-*-  (PXX  ( I ) ^•!|/.A+f’A  1 ( I J CHA»  ) ^(<.a-XBAR  ) 

XX(I)=aX(I)  + (PXx(I)  ^‘HX  V+PaY  i ( ) «.‘r  > j *.  ( t Y- Vf  AR  J 
VV(  I )=yFiAR+(PXV(  I )<--haX+dVi  ( I |*  HXY  )t(2X-Xr:AK) 

YY(  I)  = YY(  I ) + (pXY  ( I)  «HaY+E  \ Y(  I ) *HVY  ) *(  2 Y-Yc.  AR  ) 
l.alJ4-  ( pAoflHXX+rYl-'*HXY  ) ( ZX-AFA.'* ) ■*•  ( PX  J<HX Y+P  VU‘ hY Y ) » ( ZY-YbAR  J 

y = V + lPAYtnXX4.PYy*nX  Y ) ( Z A-XpA .’ ) •♦•  ( PX  Yi»HXY+3  Y V»H  Y Y J * I ZY-YBAR  1 

C CPoATE  GKlV'iiiG  fiuIoZ  BoTifATl. 

kI=FLOAT( I ) 

Caa  = CXX  + ( 1 (ZX-XitARJ  cC2-r.AX-KXA  J /TAC-CaX>/UI 


P'ig.  3— Arogram  licUng  (cootinutd) 
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CXY.CXY+  ( ( ( 2.':-xra.'v’  ) « < v-YaAH ) -g;<  y-rxy  I / tau-cxy  j /:u 

rVY.crVY+(  ( ( Z Y-YflAfi ) iJ«Z-GYY-KYY  ) / TAU-CW  J /K I 
R<*CXYiKU*U+V*V)/v,'/V 

oxx.cxx 

CYYmCVY 

IFtDXX.LT.  >.)  r>XX=u, 

1F<DYY.LT.v,I  nVY=u, 

IF<R<.GT.DXX+DYY1  RK='.  XX+OYY 
TFJRK.LT,-r:XX-r)YY!  RK=-r>AX-nYY 
oxx«,5*(nxx+nVY+RK*<o«'j-v«-v)/<uttj+v«^v)  i 
UX Y=R<«U#V/ { U*U+V*V ) 

QYY.QXX-RK.'f  (U<‘u-ViiV  ) / ( u*Li  + V»V| 

Qurpvji 

1 e onSfRVATlON  Ifi'  dX 
T « TlRfc 

(XXt  YY)  ■ CURHFnT  position  ESTIMAfE  IN  X-Y  COORDINATES 
SMAJ  « SEKINAJOR  AXIS  OF  &6  MEKCENT  CONTAINMENT  ELLIPSE  FOR 
CURRENT  POSITION 

SMIN  ■ SEM!"INOR  AXIS  OF  Ct  NTAINNEiiT  ELLIPSE 

TH  = ORIENTATION  CP  St-ii;  AJOR  AXIS  lOLO.  CLOCXuiSE  FROM  Y-AXISJ 
00  3 1=1. N 

PRINT  7*  r<  n»AX(  I)  »YW  I)  .oMAJi  I ) »Sj.m;(I)  .Th«I) 

7 F0RMAT(?jX.3F1  .2»SX.'-.Flc,2/) 

PRINT  a 

• format (20X///) 


TRACK  S-V-OTiiER 


X5(N)«XX(N) 

YS(N)«YY(N) 

NiVl=N-l 
0EN=TIN)-T( IJ 
0XX»0XX+pUU*DEN 
CXY=0XY+pUVK0EN 
uYY=^.Y^+PW*^Ul.;i 
00  <2  K=l*N:il 

I*N-K 

TaO«TU  + 1)-TU  ) 

P1=PXX(1) 

P2=PXYII ) 

P3=PYV< I J 

nE.i=(Fi+0XX*T;.U  j«  <P?+.,YY!i7A>-I-I 
HXX»P1*  I P3+0YYiiTA0 1 t P2+OXY*TaO  ’ 
HXY-P2<^<Pl+0XY»TAU)-r-l*(??+vjXY*TA0) 

HVX.P24U  P3+(,'YY»TAu  I -P ( P2+0A  Vc  T„U  ) 
HYYxP3»(pi+OXX2TAO)-r?«(P?+OXVtTAoJ 

XSI  I)xaX(I  j + (HXX*IXSi  1 + 1 )-A/i  n-s.siAU)+HAY*(Y5I  I+11-YY(IJ-V+TA<-)  1/ 
IDEM 

2 YSU)=YY(:)  + (j|YX*USi  :+l  J-AX(I  J-0»TAU»+in  Yi(YStI+H-YYlI)-V*TAU))/ 
IDEA 

SXaspxx  ({;) 

SXYxPXVlN) 

SYV.pVYiNl 

C1»SXX+SYY 

C2»S0RT  I (^XX-;  YY)**7+A,iJjAy*«2) 

C1=.SP<C1+C2) 


Fig.  S— Ttagnm  Urting  (continued) 
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C2«C1-C^ 

Si.AJ(fi)=2.*i»(.RTiCl) 
S'.>IN(N)a2,«i.cr<T(C2) 
TH(N)a57,3*ATAIi{  ( SXX-Cl  1 /iXYl 
00  5 L=2»K 
I “U—L+l 

TAU«T<  I + D-TUl 
P1»PXX( 1 ) 

P2«PXY(n 

jrtpSxpYYU  ) 

A1=P14.£JXX*TAU 

A2  = P2+C.aY«TAU 

A3=P3+0YY*TAU 

DcTaAi'KA3-»42*A2 

Hl=A3/UtT 

H2=-A2/1.'ET 

H3aAl/DET 

Bl-Pl*ril+P2*:H2 

B2=P1*H?+F?«H3 

63  = P2«H1-*.P3«H? 

•44  = P2Vh2+P3«-H3 

01*SXX-A1 

02=SXY-A2 

D3*SYY-A3 

£l=Bl*01-*-B2**i>2 

E2*ol*u2+ti2*'<-^3 

E3=E3«C)l+hA<-U2 

rAaB3*02+B4«L)3 

3XXrp^♦r1#^]+^2^^R? 

SXYsP2+Pi*P3+E26HA 

i>YYaP3+E3»l'3+EA»bA 

Cl=SXX+iiYY 

CJ'=50XT  ( (iXX-;jVYi<*2+A«<' jXY^;«-2l 
C1  = .5*{'C1+C2» 

C2=C1-C7 

Si<:AJ(I)=2.’^SUKT(CU 
o,-IN(I)=2.obUHT(C21 
A Th(  I )=3V.:-<>ata:,u.‘:Xa-cu/>-avj  + ;._, 


0‘JTPUT 

\ = observation  IhOLX 
T = Tliit 

(Xit  YS)  « S^100THE0  Pu-»ITK.i;  I.i  X-V  COORDINATES 

SJ.’AJ  » SPOINAJOK  AXIS  cP  86  HEP.CE'*!  CCNT AIN.’-iEilT  ELLIPSE  FUR 

si;oothed  position 

■ SF.‘iI.*iINOU  AXIS  oF  Cw»*TaI '^^E•iT  ♦^LLIPSE 
Th  a CRIENTATKiN  Ur  SE.-I  -AJ-a  AX*S  IOEO.  CLUCA'.«lSt  FROM  V-AXlSj 


DO  4 lai.N 

A print  7*Ti  I J tXSl  I J »Yo<  I J II  »s:  IKU)  »ThI  n 

FflO 


Fig.  3— PJrofEun  ItaUag  (oonUauad) 
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INITIAL  OBSERVATION  ANO 
POSITION  ESTIMATE. 


SIZE  OP  86%  POSITION 
CONTAINMENT  ELLIPSES  (CIRCLES! 
FOR  ALL  OBSERVATIONS -ACTUAL 
OBSERVATION  ERROR  VALUES  WERE 
ALL  ZERO. 


I X X' position  observations 

(EQUALLY  SPACED  IN  TIME) 

TRUE  TRACK 

•• • SEQUENCE  OF  CURRENT 

17  IB  POSITION  ESTIMATES  AT 

OBSERVATION  TIMES  - NUMBERED 
IN  ORDER  (KALMAN  FILTER) 


SMOOTHED  TRACK  NOT  SHOWN. 


Fig.  4— Ferformance  of  recunivc  ohip  tra clung  algorithmA  on  idealued  track 
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• Beginning  at  time  with  the  y axis  oriented  toward  local  north  and  the  x axis 
toward  local  east,  let  4>i  and  be  the  latitude  and  longitude  of  the  estimated  position  at 
that  time.  In  these  local  rectimgular  coordinates,  the  velocity  estimates  u,-,  0,-,  the  (4  X 4) 
state  covariance  matrix  P,-,  and  the  maneuvering  matrix  estimate  are  also  available.  For 
convenience  here,  denote  south  latitudes  and  west  longitudes  as  negative. 


• Propagating  the  ship’s  position  by  dead  reckoning  to  that  at  the  time  of  the 
next  observation,  adopt  a new  coordinate  system  with  origin  at  that  position 
with  y'  and  x'  aligned  along  local  north  and  east.  These  parameters  are  speciri^  by  the 
equations 


«■  = - ti 

(48) 

f = >/uf  + of 

(49) 

fr 

7 = 5“  ; Rg  = earth  radius 
•“e 

(50) 

(51) 


fsin  8 = 


Uj  sin  y 

^COS0j  + i 


cos  8 


cos  0,-  cos  7 - y sin  0,-  sin  7 


COS0j 


♦1 


> 


(52) 


Take  fi£[0, 2H) 

= Vl'i  + 5 

.if  - 2n. 


(53) 


• Ckimpute 


Oj  cos  0j  cos  7 - f sin  0j  sin  7 

rcos?,>l 


\ 

V 
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“i+1  = “i 


cos 


CO«^i  + l 

These  are  the  values  of  v,  i!  at  time  in  local  rectangular  coordinates. 
• Define  the  rotation  matrix  as 


^.>1  = J2 


— — — ^ 

vA*!  - ! 5,-5;+i  + 


• 

— “ 

1 “i 

"i+i  I -“i+i 

f 1 f 

f 1 f 

A A 

-U,-  1 W,- 

“i-M  I ‘'•>1 

_T  1 7 ^ 

_ "T  1 T _ 

• Compute  covariance  propagation  equation  tor  rectangular  coordinates, 

Eqs.  (30)  through  (34)  and  (12)  throu^  (21)  for  this  motion  model 


• Rotate  to  updated  coordinates: 


®l+l 

0 ' 

'07*1 

0 

0 

0 

®/+i 

• Set 


^i+l  ~ 


“ 0 
yj+1  = 0- 


values  of  x,  y 
at  time  f,>i  in  local 
coordinates 
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• Compute  with  Eqs.  (22)  through  (24)  and  (27)  through  (34) 


estimated  values  at  time  in 
' local  rectangular  coordinate 


yi*l 
“i*l 
"i+1 

’J 

Compute  latitude  and  longitude  of  (updated)  position  estimate  at  time 


^i+l  = ^i+l  * 


yj*i 

Re 


= J'i+l  + ^ cos^i+i 


— End  of  Procedure  — 


Track  Smoothing 

If  the  preceding  track  generation  procedure  has  been  followed  through  the  ^th  ob- 
servation time,  there  are  ^ 1 observation  times  f,-,  t = 0, N,  with  corresponding 

Kalman  filter  estimates  of  latitude  arid  longitude  and  The  Bayesian  smoother  can 
be  implemented  in  this  context  by  computing  **smoothed"  offitets  d(  to  these  position 
estimates,  and  their  cotrteponding  error  covariance  matrices  Kf.  These  ofbets  are  2-vectors 
in  linear  distance  units  aliped  with  local  north  (i.e.,  the  unit  vector  points  north  at 
rj/j).  These  offiKt  and  covr  'Mice  matrices  can  be  computed  recursi^y  as  follows. 

• Denote  the  current  (at  time  t^)  positive  semidefinite  estimate  of  Q by  Qjf  and 
the  estimate  of  the  velocity  vector 


u 

V 

by  Uff.  These  variables  are  all  eqiressed  in  the  local  rectangular  coordinate  frame  of  observa' 

tiontimef/v 
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• Set  dff  - 0,  Kf]  - Pjf 

• Loop  through  the  following  steps  for  i = N - 1,  N - 2,  0. 

1.  Rotate  the  offtet  vector  and  its  error  covariance  matrix  to  the  local 
coordinate  frame  at  ipi  by  computing 


and 


where 


a,-  is  the  rotated  offtet  vector 

Li  is  the  rotated  (2X  2)  covariance  matrix 

is  the  rotation  matrix  defined  in  the  previous  subsection  for  the  Kalman 
filter  (which  could  also  be  obtained  from  <j>i,  ^,-4.1  to  within  the 

degree  of  approximation  adopted  here). 


2.  Rotate  the  velocity  vector  to  the  ith  local  coordinate  frame  by  computing 


“i  = • 

Compute  and  from  0,-  and  \lii  with  Eqs.  (48)  through  (53),  except  that  Ci  and 
Vj-  are  replaced  by  the  components  of  u^. 


3.  Let 


bi  = 


Rf  cos  ^i+i(^',*i  - <^,+i) 


^€(^1*1  “ ^i*l) 


4.  Compute 

^i+i  = Pi  * . where  Q,-  = and  = tg^i  - f,- 


5.  Compute 


and 


d.-  = Wii(a,-6,.) 

Ki  = i*  + PiM;li(Li~Si^i)M;liPi 


28 


NRL  REPORT  7969 


which  are  the  offset  and  the  smoother’s  corresponding  error  covariance  matrix  for  the  ship 
position  at  the  <th  observation  time,  expressed  in  the  tth  local  coordinate  frame.  The 
component-by-component  details  of  this  computation  are  not  shown  here,  but  are  analo- 
gous to  Eqs.  (43)  through  (47). 


— End  of  Loop  — 

If  desired,  the  latitude  and  longitude  and  of  the  smoothed  position  estimate  at 
time  tf  can  be  computed  as  follows. 


4>i  + 


dj 

Rg  cos 


TRACKING  ON  A SPHERE:  RECTILINEAR  COORDINATES 

Ship  tracking  on  a sphere  is  sometimes  performed  in  terms  of  three-dimensional 
coordinates  instead  of  geographical  latitude  and  longitude.  The  use  of  such  a coordinate 
system  enables  the  computational  effort  to  be  reduced  considerably,  largely  by  the  avoid- 
ance of  trigonometric  computations,  at  the  expense  of  a slight  increase  in  storage  require- 
ments. This  section  develops  an  adaptation  of  the  above  track  generation  and  smoothing 
algorithms  for  a spherical  earth  in  these  rectilinear  coordinates.  This  particular  adapta- 
tion circumvents  the  potential  problem  of  singular  covariance  matrices. 

In  this  system  a target’s  position  on  the  earth’s  surface  is  described  by  a vector 


r 


whose  components  are  the  coordinates  of  the  target’s  position  in  an  earth-centered  rec- 
tangular coordinate  system.  Specifically,  the  x axis  is  taken  as  interseccing  the  equator 
at  zero  longitude,  the  y axis  as  intersecting  it  at  90°E  and  the  z axis  as  aligned  along  the 
North  Pole  (a  right-handed  coordinate  system).  Boldface  lower  case  letters  are  used  here 
to  denote  such  3-vectors. 

Motion  along  a great-drcle  path  is  represented  by  a vector  normal  to  the  plane  of  the 
great  circle.  The  magnitude  of  this  vector  is  the  angular  speed  of  the  motion  and  its  sense  is 
such  that  eastward  motion  along  the  equator  is  represent^  by  a vector  aligned  with  the  North 
Pole.  Hence,  the  motion  of  a target  at  position  r and  velocity  r is  represented  by  a vector 


<t) 


such  that  f = cjX  r. 
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The  txack  of  a target  is  generated  recursively  from  a sequence  of  noisy  observations 
Zj,  i = 0, 1 ...»  of  the  target’s  position  r(t,-)  in  the  rectilinear  coordinate  system.  Also 
given  with  each  observation  z,-  is  a symmetric  2X2  covariance  matrix  fZ,-  for  the  compo- 
nents of  the  observation  error  vector  z,-  - r(t,-)  in  the  directions  of  local  east  and  local 
north.  These  components  of  z,-  could  easily  be  computed  by  the  usual  spherical  coordi- 
nate formulas  from  a position  report  of  geocentric  latitude  and  longitude. 

WARNING:  Because  of  the  earth’s  oblateness,  the  geocentric  latitude  differs  from 
the  more  commonly  used  geodetic  latitude  (based  on  the  orientation  of  the  local  horizon) 
by  about  10  mi  at  middle  latitudes. 

Track  Generation 

For  a particular  target,  the  above-mentioned  track  generation  algorithm  can  be  im- 
plemented in  this  rectilinear  coordinate  system  by  computing  and  storing  estimates  of  r 
and  ro,  a 4 X 4 symmetric  covariance  matrix  P for  the  errors  in  this  estimate,  and  an  esti- 
mate of  a 2 X 2 symmetric  driving  noise  matrix  Q after  the  receipt  of  each  successive  ob- 
servation. The  components  of  the  P and  Q matrices  refer  to  the  local  east  and  local  north 
components  of  the  errors  in  position  and  velocity  and  the  driving  noise.  For  a time  t be- 
tween observation  times,  the  target’s  position  is  estimated  by  dead  reckoning  (along  a great 
circle  path)  from  its  estimated  position  and  velocity  at  the  time  t,-  of  the  last  observation. 
Hence,  using  the  circumflex  to  denote  estimated  v^ues, 

«(f,)  X Ktf) 

Ht)  = r(t,)coa((t-f,)|w(f,)ll  + 8inl(t-f,)l«(t,.)ll 

W(f)  * w(fj) 


If  desired,  the  value  of  the  P matrix  can  be  computed  for  such  an  intermediate  time  as  it 
is  in  the  section  ‘^Tracking  of  Planar  Motion.”  The  x subscripts  there  correspond  to  local 
east  components  here,  and  the  y subscripts  correspond  to  local  north  components. 


The  item  of  major  importance  is  the  manner  in  which  the  estimates  of  r,  u;,  and  Q, 
and  the  covariance  matrix  P are  updated  immediately  after  each  observation.  We  initiate 
this  process  immediately  after  the  initial  observation  Zq  by  setting 


^0  “ *0  vector) 

<Do  = 0 (3  vector) 

Qq  = 0 (2X2  matrix) 


Pn  = 


I a 
l 


2 j 0 


(4X4  matrix) 
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where  (q  denotes  r(fo),  etc.,  and  is  an  externally  specified  prior  (linear)  velocity  com- 
ponent variatice.  ITie  general  updating  step  begins  immediately  after  obse^tion  time  f,- 
with  the  estimates  f,.,  <o,.,  Q.^  and  the  covariance  matrix  Pf.  The  components  of  the  mat- 
rices i*-  and  Qj-  are  denoted  for  convenience  as  follows: 


Pi  = 


1 11- 

Pee  I Pen  I Pee  I Pen 

''v 1 f 1 

'‘i.  Pnn  ! Pne  { Pnn 

' T 

Symmetric  ''  s Pen 

...a  , 


Qi  = 


I 

|\9«c  I Qen 
\ I ^rm 


where  e and  n stand  for  local  east  and  north,  respectively.  When  the  next  observation 
is  received  at  time  the  following  proc^ure  can  be  used  to  compute 
the  updat^  quantities  rj+j,  Q,+i,  and 

1.  Compute  the  quantity 


It  is  then  possible  to  express  the  components  of  the  local  east  unit  vector  e and  the  local 
north  unit  vector  n (at  location  fj)  as 


It  is  not  necessary  to  compute  these  unit  vector  components  separately,  but  they  are  used 
in  deriving  some  of  the  following  steps. 

2.  Compute  the  local  east  and  local  north  components,  u,-  and  t)|-,  of  the  estimated 
velocity  at  as  (o),  X f,)  • o,-  and  (w,  X f,)  • n,-;  this  yields 

A 

“j  = OiTi  “ •5:  («i*i  . (east  component) 


(north  component) . 


3.  Compute 


such  that 


where 
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~ ~ r,) 

+ l = r,  cos(Th,)  + — sin  (Tb,-) 


bj  = y/af  + If  + 7?  = IwJ 

T = - ti. 

This  is  position  propagation  by  dead  reckoning  along  a great-circle  path.  As  a simplifying 
approximation,  one  could  compute  instead 


ff+i  = n ~ ^ ^ If)  • 


4.  Compute  the  new  value 

®i-fl  ~ V^i+1  y/  + l • 

5.  Compute  the  new  local  east  and  local  north  velocity  components  as  hi  step  2, 


pving 


A A 


‘'.•*1  = \ 5171 ] 


6.  Compute  the  rotation  matrix  such  that 


= A 


1 j ^ w,«,+i 

_ ^ 

* + l + l ' w.wi  + i + UiU,*i 
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7.  Compute  a positive  semidefinite  approximation  to  Q^;  denote  it  by 


' * 1 
' Qin 

= 

— 

X 

#5 

o* 

\ 

I 

* § 

Qi  = 


One  reasonable  method  of  doing  this  is  described  in  the  section  “Tracking  of  Planar 
Motion.” 

8.  Compute  the  symmetric  4X4  matrix  A/ such  that 


M = 


m 


ee 

s 


men  [ ^ee 


_ I _ 
^nn  I ^ne 
+ 


! Pie 


m. 


m. 


Pen 


Symmetric 


I n •• 
V , t'nn 


and 


ihee 

Pee 

+ 

+ Pee'f^  + 

^en 

* 

Pen 

+ 

(Pen  *Pne)^  + Peh'^^  * 

^nn 

Pnn 

+ 

^Pnn'T  * Pnh'^^  + 

Ke 

= 

Pee 

+ 

Pee^ 

^eh 

= 

Pen 

+ 

Peh"^ 

^ne 

= 

Pne 

+ 

Peh'T 

^nn 

- 

Pnh 

+ 

Pnh^- 

9.  Compute  the  rotated  matrices 


M = 


®i+l 

! " 
< 0 

0 

M 


0 


(4X  4) 


and 


^i+l  = ®j+iQi®i>l  • 

Denote  their  components  with  analogous  subscripts. 


(2X  2) 
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10.  Compute  the  local  east  and  local  north  components  and  e„  of  the  residual 
vector 


Ui+i'W  = 


by  taking  dot  products  with  the  unit  vectors  and  (not  shown)  used  in  step  2, 
which  gives 


and 


11.  Compute  the  new  state  covariance  matrix  as 


Pi*i  = 


Pee 

1 

1 

Pen  ; 

Pee 

I Pen 

’p 

Pen 

1 

Pnn  [ 

Pne 

1 Pnn 

Pee 

1 

1 

V 

Pne  1 

Pei 

1 Pen 

- — - 

• H- 

4- 

— 

,4. 

Pen 

1 

1 

Pnn  1 

Pen 

1 Pnn 

^,>1  - 


-i*+i 


m^g  I ^en 
1 

"»en  1 "»m. 


trig^  ^ 

^eh  I ^nn 


*€« 


m. 


1 - 

\-l 

• 1 

1 

) — 

1 "*e« 

Pi*l 

"fee  1 

"fen  1 

"fee  1 "fen 

4 

1 

+ 

+ 

4 

1 ^nn 

J 

"fen  I 

"fnn  I 

"fne  ] "fnh_ 

12.  Compute  Kalman  filter  correction  terms  5 in  local  east  and  local  north 
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13.  Use  the  Kalman  filter  correction  terms  to  compute  the  new  estimates 
and  w,+i,  the  components  of  which  are 

yj+i  “ ^ • ’ ^n) j ■*■  yj+i 


— 4.  ~ 


“i+l 

= "i 

y/+i5« 

II 

1 

A 

. c 

= ii 

• 

Also,  it  might  be  wise  to  renormalize  + i so  that  its  magnitude  is  Rg  to  prevent  an  ac- 
cumulation of  roundoff  errors. 

14.  Compute  the  new  estimate  of  the  driving  noise  matrix  Qi+i,  as  Qi^i  = 0 if 
i > 0,  otherwise  as 


— 1 / 1 ~ ^en  1 _ 

Qi*l  = + J~Tl  It  t~2 

\ ~ ^en  \ ^nn 


— End  of  Update  Cyde  ~ 


Simplifying  Approximations 

It  mi^t  be  possible  to  reduce  the  computation  required  to  implement  this  tracking 
algorithm  with  little  sacrifice  in  accuracy  by  adopting  some  approximations.  In  addition 
to  that  mentioned  in  step  3 for  generating  from  f,-  and  to,-,  another  likely  approxima- 
tion is  to  neglect  the  rotation  of  local  east  and  local  north  between  successive  observations. 
With  this  approach  it  is  possible  to  omit  steps  1,  2,  6,  and  9 in  the  update  cyde  by  the 
use  of  ^ Afj+i  in  step  11  and  Q,-+i  = Q,-  in  step  14. 
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Track  Smoothing 

The  basic  approach  here  is  the  same  as  in  pages  27  to  29.  The  smoother  is  imple- 
mented as  a backward  recursion  to  compute  offsets  d,-  to  the  Kalman  filter  position  esti- 
mates r,-  and  also  the  smoothed  error  covariance  matrices  K,-  corresponding  to  the  d,-.  The 
offtets  are  computed  in  local  east  and  north  coordinates;  thus  the  d,-  are  2-vectors  and  the 
iTj  are  2 X 2 matrices.  If  smoothing  is  to  take  place  immediately  after  time  it  is  as- 
sumed that  the  Kalman  filter  outputs  cjfj,  Qff,  r,-,  and  Pi,  i = 0,  N,  are  all  available. 
The  procedure  can  be  implement^  as  d^ciib^  below. 

• Set 


djv  = 0 

Qn  = Qn‘ 

• Loop  through  the  following  steps  foii~N-  1,  N - 2,  ...,  0. 

1.  Follow  steps  1 through  5 of  the  filter  update  cycle  of  the  preceding  subsection, 

except  with  o),-  replaced  by  tejy  to  compute  d,+i,  ^•+i»  e,-  and  n^. 

2.  Rotate  the  offtet  vector  di+i  and  covariance  matrix  Ki^.i  to  the  ith  local  Carte- 
sian coordinate  fiame: 


Oi  = (offtet  2-vector) 

~ (2X2  covariance  matrix) . 

3.  Compute  the  local  east  and  north  components  of  the  I'th  coordi- 

nate frame:. 

= (f,+i  - r,+i)  ‘ e,- 

= (f,-+i  - 7i+i)  • n,- . 

4.  Compute 

^1+1  = + CjT,  , 

where 
Qi  = 
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5.  Compute  the  new  offset  and  covaziance  matzix: 


n-l 


w-1 


— End  of  Loop  — 

At  this  point,  one  could  also  compute  the  smoothed  position  vectom  r,-  as 

li  = f.  + [e,-  I n,-]  di , 


:e 


[e,-  J Hj-  ] denotes  a 3X  2 matrix. 


Appendix  A 

GENERAL  RECURSIVE  FILTER  AND  SMOOTHING  ALGORlTHBfS 


Suppose  that  x it  a state  vector  describing  a ship’s  motion  in  the  context  of  Eqs.  (1), 
(2),  and  (3),  and  that  the  corresponding  Kalman  filter  is  being  used  to  track  the  ship. 

The  conditional  moments  rj  and  K can  be  computed  recursively  from  the  data  by  means 
of  the  following  equations. 


Forward  Equatiotu  (Kalman  Filter  for  Eqs.  (1)  Through  (3)) 
X = Fx  + w 
P = FP  PF'^  + Q 


S between  observations 


P(ti)  = P(to)  = Mo 

» P(f|-)  - Hf[HiP(t;)Hf  *Ri\-%P(t;) 

£(ti)  “ iUr)  + P(ti)HfRi^[Zi-  HiX(t;n;  xito)  « Xq 


(Al) 

(A2) 

(A3a) 


at 


y observation  (A3b) 
time 

(A4) 


For  the  ’Tlat  prior”  case,  use  P''^(tQ)  = 0 in  £q.  (A3a). 


Backward  Equations  (Bayesian  Smoother  for  Eqs,  (1)  Through  (3)) 


X = Fx  + w 
P = FP  * PF"^  + Q 


between  observations 


(A5) 

(A6) 


P(tn  = [P'^(0  - ajRl^HiY^ 

= P(tt)  + P(ti)Hf[Ri  - HiP{it)Hf]'^HP(tt) 
£(0  “ £{ti)  - P(ti)HfRT^lZi  - HiX(tt)] 


(A7a) 

at 

observation  (A7b) 
time  f,- 

(AS) 
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The  Kalman  filter  statistics  jc(t)  and  P(t)  ate  computed  using  only  the  forward  equations 
firom  time  tg  to  time  t.  The  moments  rj{T,  t)  and  K(T,  t),  for  7 > t,  are  computed  by 
first  obtaining  x(T)  and  P(7)  using  the  forward  equations  from  time  fg  to  time  T.  Then 
these  values  are  used  as  boundary  conditions  at  ti^  7 for  the  backwa^  equations,  which 
are  then  used  recursively  from  time  7 to  time  t to  obtain  t}(7,  t)  and  K(T,  t). 

These  results  are  developed  in  greater  detail  in  Bryson  and  Ho.* 


*A.E.  Bryaonand  Y.C.  Vo,AppUtd  Optimal  ConO’oi,  Bkiadell,  Walthaca,  Mass., 1968. 
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Appendix  B 


INCLUSION  OF  OCCASIONAL  VI 


rY  OBSERVATIONS 


In  the  context  of  the  pUnir  motion  of  the  section  “Traddnx  of  Planar  Motion,*’ 
suppose  that  a noisy,  independent  measuremmit  of  the  ship’s  velocity  is  obtained  at  ob> 
sen^on  time  t(  in  additicm  to  the  position  measurement  Zj.  We  that  this  velocity 

measurement  can  be  adequate!^  described  as 

= + (Bl) 

where 

- measured. velocity  (2-vector) 

s = actual  instantaneous  velocity  in  x-y  coordinates  (2-vector) 

a,‘  = zero-mean.  Bivariate  Normal,  random  variable  with  covariance  matrix 

Unfortimately,  the  ship  motion  model  (pa^  4 and  5)  is  an  inadequate  approximation  to 
the  actual  motion  for  the  purpose  of  dealing  with  such  velocity  observations  because  an 
instantaneous  velocity  cannot  be  defined  for  the  Brownian  motion  component.  The 
model  can  be  refined  for  this  purpose,  however,  by  specifying  a probability  distribution 
for  the  difference  between  the  average  velocity  and  the  observed,  instantaneous  velocity. 
One  convenient  way  of  doing  this  is  to  assume  that  these  differences  are  statistically  in- 
dependent for  different  observation  times  and  distributed  with  a zero-mean  Bivariate 
Normal  distribution  with  covariance  matrix  Df  at  observation  time  if.  As  a default  proce- 
dure to  avoid  specifying  Df  externally,  it  might  be  reasonable  to  use 


Qxx  1^  9jry 

L 1 J 


vriiere  q^,  and  qyy  are  as  defined  in  Eqs.  (30)  throu^  (34).  This  procedure  is  based 
on  matching  the  variances  of  the  observed  fluctuations  in  position. 

With  this  refinement,  it  follows  from  Eqs.  (4)  and  (Bl)  that  the  velocity  measurement 
if  can  be  expressed  as 


_vJ\  LyJ  I 


(B2) 
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the  latter  term  in  parentheaei  being  a Bivariate  Normal  random  variable  with  mean  zero 
and  covariance  matrix  (Sj  * Dj).  Thatefot^  the  coihpoaite  obMfvation  vector 


can  be  regarded  at  a noisy  obaervation  of  the  atate  vector 

r * ~ 

■r  y 

I u 

L «- 


at  time  t,-  in  the  context  of  Appendix  A.  The  (4  X 4)  covariance  matrix  of  the  observa- 
tion noiae  rif  in  thia  context  is 

- ' to  iti  + 


Specializing  the  results  of  Appendix  A to  this  particular  case  shows  that  the  track 
genoation  procedure  (pages  6 tc  14)  applies  here  also,  except  that  Eqs.  (22)  through  (24) 
are  replaced  respectivriy  by  the  following  three  equations  whenever  a velocity  measure- 
ment is  obtained  at  time  : 


*j+i 

Pxx  1 Pxy 

z^(i  + 1)  - X, 

- TUi~ 

— 

ZZ 

— 

+ T 

— — 

+ 

n * n 

^PjCv  5 Pyy  _ 

-^i+l 

— 

A 

.y*-- 

A 

i+1 

_Zy(i  + 1)  - y,- 

- TVi_ 

~Pxu  1 

. 

Pxv 

- (z:.-.!  ti-i  - 

"“i" 

Jyu  > 

Pyv  _ 

V 

_*'i_ 

(B3) 
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‘ “i*r 

u,- 

Pxu  1 Pyu 

'z^U  + 1)  - Xi  ~ ntf 

— 

= 

— — 

+ 

1 

■^i+1 

. ^*>1- 

Jk 

_ Pxu  j Pyv  _ 

> 

Zy(i  + 1)  - Vi  - n),_ 

Puu  I Puv 
LPmu  ! Puv  J 


i+1 


u*'.- 


(B4) 


i»  +l  = Af  - Af  Mf  + 


(B5) 


The  Af  matrix  components  are  as  defined  in  Eqs.  (12)  through  (21).  Since  the  track 
propagation  steps  are  unchanged,  there  are  no  additional  modifications  foor  velocity  ob- 
seri^ons  in  the  spherical  track  propagation  algorithms  because  the  track  updating  steps 
are  performed  in  Cartesian  coordinates  there  also. 

The  results  of  Appendix  A can  also  be  specialized  to  this  case  for  track  smoothing. 
It  would  be  necessary  to  retain  all  four  components  of  the  state  vector  for  this  purpose 
because  velocity  observations  must  be  taken  directly  into  account.  This  would  be  a dras- 
tic departure  from  the  other  smoothing  algorithms  developed  in  this  report,  which  are 
based  on  a two-component,  position-only,  state  vector  approximation.  Therefore  the  de- 
tails of  this  four-dimensional  smoother  are  not  developed  here.  A two-dimensional 
smoother  can  always  be  used,  anyway,  by  ignoring  the  velocity  measurements. 
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Appendix  C 

DEVELOPMENT  OF  CONSTRAINT  ON  MANEUVERING  MATRIX  ESTIMATE 


A procedure  is  described  in  Eqs.  (30)  throuj^  (34)  for  modifying  the  estimates 

and  QyyU)  nf  the  maneuvering  matrix  components  to  ensure  that  the  re- 
sulting estiTOtes  form  a positive  semidefinite  matrix  whose  principal  axes  are  aligned  with 
the  current  estimate 


““i‘ 

."i. 


of  the  average  velocity  vector.  The  justification  for  this  particular  procedure  is  discussed 
below. 

Suppose  that  the  estimated  average  velocity  vector  is  as  shown  in  Fig.  Cl,  where  the 
( and  c axes  are  in-track  and  cross-track  coordinates.  Ftom  this  figure,  the  formulas  for 
rotation  of  coordinates  give 


"cos  d , -sin  0"|  Pi” 

1 — 

_sin  6 J cos  0 J L ^ _ 


from  which 


cos2  6 = „ * - u . 

uf  + Of 


sin^  6 = a -- — ^ . 

-2  . <^2 
uf  + vf 


sin  B cos  6 = 


->2 

uf  + vf 


For  a maneuvering  noise  covariance  matrix  which  has  the  diagonal  form 
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Sind 


and 


sin  d « 


from  which  we  get 


and 


sin  d .. 

9xy 

cosd 

aind  cosd 

sin  d . 

7*y 

cosd 

sind  cos  ’ 

9xy 

sin  d cosd_ 

Qxy 

sin  d cos  d_ 

(C6) 


(C7) 


For  arbitrary  4jcjc»  9*y»  aod  9vy»  (Cl)  through  (C3)  can  be  used  in  modifying  Eqs. 
(C6)  and  (C7)  as  follows  so  wst  the  diagonal  matrix  (C4)  is  always  positive  semidefinite: 


<li  = I « + X) 
1 


where 


(€-X), 

^ = max  {0 , (q^  + gyy)} 
* -2\ 


(C8) 


i if 


X = ^ if 


UiVi 

Qxyifif  + vf) 


UiVi 


< 


(C9) 


9xy 


uf  + of 


UiVi 


otherwise , 


Using  Eqs.  (Cl)  through  (C3)  in  covariance  matrix  (C5)  for  this  modified  matrix  in  x,  y 
coordinates  gives 
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